/*
 * RiconoscimentoPorta.c
 */

#include "../../include/vision/RiconoscimentoPorta.h"


RiconoscimentoPorta::RiconoscimentoPorta(){
}


Rect RiconoscimentoPorta::riconoscimento_porta(Mat rgbMat, Mat img_porta_filt2){
	Mat pointsfPorta;
	vector<vector<Point> > contours_porta;
	int ii;
	Rect maxRettangoloPorta=Rect(0,0,0,0);
	Point centroRettangoloPorta;
	bool portaRiconosciuta=false;



	findContours(img_porta_filt2, contours_porta, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

	vector<vector<Point2f> > contours_poligono_porta( contours_porta.size() );
	vector<Rect> rettangoli_porta( contours_porta.size() );

	for(ii=0;ii<contours_porta.size();ii++){
		Mat(contours_porta[ii]).convertTo(pointsfPorta, CV_32F);
		approxPolyDP( pointsfPorta, contours_poligono_porta[ii], 3, true );
		rettangoli_porta[ii] = boundingRect( Mat(contours_poligono_porta[ii]) );
	}


	for(ii=1;ii<contours_porta.size();ii++){
		if(rettangoli_porta[ii].area()>maxRettangoloPorta.area() && rettangoli_porta[ii].area()>MIN_AREA_PORTA){
			portaRiconosciuta=true;
			maxRettangoloPorta=rettangoli_porta[ii];
		}
	}

	if(portaRiconosciuta){
		rectangle( rgbMat,maxRettangoloPorta,Scalar(0,255,255),3);
		centroRettangoloPorta=Point(maxRettangoloPorta.x+maxRettangoloPorta.width/2,maxRettangoloPorta.y+maxRettangoloPorta.height/2);
		circle( rgbMat, centroRettangoloPorta, 3, Scalar(0,255,255), -1, 8, 0 );
	}

	return maxRettangoloPorta;

}

double RiconoscimentoPorta::calcoloProfonditaPorta(Mat rgbMat, Mat depthMat,Rect maxRettangoloPorta, IplImage* img){


	unsigned short profonditaPorta;
	char profonditaPortaString[20000];
	int ii,jj;
	unsigned char h,s,v;
	Point centroRettangoloPorta;

	double cont=0;
	long somma=0;
	double profMedia=0;

	for(ii=0;ii<img->width;ii++)

		for(jj=0;jj<img->height;jj++){

			h=((unsigned char*)(img->imageData+img->widthStep*jj))[ii*img->nChannels+0];
			s=((unsigned char*)(img->imageData+img->widthStep*jj))[ii*img->nChannels+1];
			v=((unsigned char*)(img->imageData+img->widthStep*jj))[ii*img->nChannels+2];

			if((h>=MIN_H_PORTA && h<=SUP_H_PORTA) && (s>=MIN_S_PORTA && s<=SUP_S_PORTA) && (v>=MIN_V_PORTA && v<=SUP_V_PORTA)){
				profonditaPorta=((depthMat.at<unsigned short>(Point(maxRettangoloPorta.x+ii,maxRettangoloPorta.y+jj))));
				somma=somma+profonditaPorta;
				cont++;
			}

		}

	profMedia=somma/cont;
	centroRettangoloPorta=Point(maxRettangoloPorta.x+maxRettangoloPorta.width/2,maxRettangoloPorta.y+maxRettangoloPorta.height/2);
	sprintf(profonditaPortaString,"%.2f mm",profMedia);
	putText(rgbMat, profonditaPortaString, Point(centroRettangoloPorta.x,centroRettangoloPorta.y), 1, 2, Scalar(0,255,255),1,5,false);
	return profMedia;
}



double RiconoscimentoPorta::calcoloAngoloPorta(Mat rgbMat,CvPoint centroRettangoloPorta){

	char angoloPortaString[10000];
	CvPoint cPorta;
	CvPoint ptPorta = cvPoint(rgbMat.cols/2,rgbMat.rows);
	cPorta=centroRettangoloPorta;

	//Applicazione del teorema dei seni
	int aPorta, bPorta;
	double dPorta, alfaPorta;
	bPorta = abs((ptPorta.y) - cPorta.y);
	aPorta = abs((cPorta.x) - ptPorta.x);
	dPorta = sqrt((aPorta*aPorta)+(bPorta*bPorta));
	alfaPorta = asin (aPorta/dPorta);

	//Assegnazione del segno all'angolo a seconda del quadrante in cui si trova
	if((cPorta.x) < rgbMat.cols/2)
		alfaPorta = (360 * (-1)*alfaPorta)/6.28;
	else
		alfaPorta = (360 * alfaPorta)/6.28;

	sprintf(angoloPortaString,"%.2f deg",alfaPorta);
	putText(rgbMat, angoloPortaString, Point(centroRettangoloPorta.x,centroRettangoloPorta.y+30), 1, 2, Scalar(0,255,255),1,5,false);

	return alfaPorta;


}


RiconoscimentoPorta::~RiconoscimentoPorta(){

}





